﻿using System.Collections.Generic;
using System.Linq;
using Duality;
using Duality.ColorFormat;
using Duality.Resources;
using Duality.VertexFormat;
using OpenTK;
using OpenTK.Graphics.OpenGL;

namespace ClientApp.Avoidance
{
	public class HRVO
	{
		protected const float CULLING_TIME_WINDOW = 200000.0f;

		protected Agent self;
		protected Vector2 newVelocity;
		protected List<SingleVO> vo = new List<SingleVO>();
		private List<Vector2> velocities = new List<Vector2>();

		private ISet<Agent> ignoredAgents = new HashSet<Agent>();

		public HRVO(Agent self)
		{
			this.self = self;
		}

		public void update()
		{
			newVelocity = new Vector2();

			var orderedAgents = AiManager.Instance.Agents.OrderBy(c => (c.GameObj.Transform.Pos - self.GameObj.Transform.Pos).LengthFast);

			// generate velocity obstacle
			vo.Clear();
			foreach (Agent agent in orderedAgents)
			{
				if (vo.Count >= 6)
					break;
				if (agent == self || ignoredAgents.Contains(agent))
					continue;
				if (canCollideWithinTimeWindow(agent, CULLING_TIME_WINDOW))
					vo.Add(new SingleVO(self, agent));
				else
					break;
			}
			// calculate set of possible velocities
			List<Vector2> unfiltedVelocities = new List<Vector2>();
			unfiltedVelocities.Add(self.PreferedVelocity);
			// :: based on intersections
			for (int iA = 0; iA < vo.Count; iA++)
			{
				for (int iB = iA + 1; iB < vo.Count; iB++)
				{
					vo[iA].intersect(vo[iB], unfiltedVelocities);
				}
			}
			// :: based on velocity projections
			foreach (SingleVO singleVo in vo)
			{
				singleVo.project(this.self.PreferedVelocity, unfiltedVelocities);
			}
			// prune velocities which lie inside the velocity obstacle
			velocities.Clear();
			foreach (Vector2 velocity in unfiltedVelocities)
			{
				bool valid = true;
				foreach (SingleVO singleVo in vo)
				{
					if (singleVo.containsVeloctiy(velocity))
					{
						valid = false;
						break;
					}
				}
				if (valid)
					velocities.Add(velocity);
			}
			// calculate best velocity
			// This is only dummy code - Replace with something robust
			float bestScore = 2.0f * self.PreferedVelocity.LengthFast;
			foreach (Vector2 velocity in velocities)
			{
				float currentScore = (this.self.PreferedVelocity - velocity).LengthFast;
				if (currentScore < bestScore)
				{
					bestScore = currentScore;
					newVelocity = velocity;
				}
			}
			if (newVelocity.Length > 0.01f)//self.PreferedVelocity.LengthFast < newVelocity.LengthFast)
			{
				newVelocity *= self.PreferedVelocity.LengthFast / newVelocity.LengthFast;
			}
		}

		public void renderDebugOutput(IDrawDevice device)
		{
			float velocityFactor = 100.0f;
			if (DualityApp.ExecContext == DualityApp.ExecutionContext.Editor)
				return;
			List<VertexC1P3T2> vertices = new List<VertexC1P3T2>();
			// render vo
			ColorRgba color = new ColorRgba(255, 255, 0, 100);
			foreach (SingleVO singleVo in vo)
			{
				Vector3 position = self.GameObj.Transform.Pos + new Vector3(singleVo.Offset * velocityFactor);
				Vector3 endLeft = position + new Vector3(singleVo.Left * 100.0f);
				Vector3 endRight = position + new Vector3(singleVo.Right * 100.0f);

				float zValue = self.GameObj.Transform.Pos.Z + 20.0f;
				position.Z = endLeft.Z = endRight.Z = zValue;

				vertices.Add(new VertexC1P3T2(position, new Vector2(), color));
				vertices.Add(new VertexC1P3T2(endLeft, new Vector2(), color));

				vertices.Add(new VertexC1P3T2(position, new Vector2(), color));
				vertices.Add(new VertexC1P3T2(endRight, new Vector2(), color));

				vertices.Add(new VertexC1P3T2(endRight, new Vector2(), color));
				vertices.Add(new VertexC1P3T2(endLeft, new Vector2(), color));
			}

			// render calculated velocity
			ColorRgba velocityColor = new ColorRgba(255, 255, 255, 100);
			vertices.Add(new VertexC1P3T2(self.GameObj.Transform.Pos, new Vector2(), velocityColor));
			vertices.Add(new VertexC1P3T2(self.GameObj.Transform.Pos + new Vector3(newVelocity * velocityFactor), new Vector2(), velocityColor));

			// render velocity set
			foreach (Vector2 velocity in velocities)
			{
				Vector3 dot = self.GameObj.Transform.Pos + new Vector3(velocity * velocityFactor);
				Vector3 hOffset = new Vector3(10.0f, 0.0f, 0.0f);
				Vector3 vOffset = new Vector3(0.0f, 10.0f, 0.0f);
				vertices.Add(new VertexC1P3T2(dot - hOffset, new Vector2(), velocityColor));
				vertices.Add(new VertexC1P3T2(dot + hOffset, new Vector2(), velocityColor));
				vertices.Add(new VertexC1P3T2(dot - vOffset, new Vector2(), velocityColor));
				vertices.Add(new VertexC1P3T2(dot + vOffset, new Vector2(), velocityColor));
			}
			device.AddVertices(Material.SolidWhite, BeginMode.Lines, vertices.ToArray());
		}

		private bool canCollideWithinTimeWindow(Agent other, float time)
		{
			float maximumRelativeSpeed = self.PreferedVelocity.LengthFast + other.PreferedVelocity.LengthFast; // [units / time]
			float distance = (self.GameObj.Transform.Pos.Xy - other.GameObj.Transform.Pos.Xy).LengthFast; // units
			return (distance / maximumRelativeSpeed) <= time;
		}

		public OpenTK.Vector2 NewVelocity
		{
			get { return newVelocity; }
		}

		public ISet<Agent> IgnoredAgents
		{
			get { return ignoredAgents; }
			set { ignoredAgents = value; }
		}
	}
}
